// 传统去噪算法效果
#include <vector>
#include <complex>
#include <cmath>
#include "Audio_RT_FFT.h"
#include "Audio_RT_Process.h"
#include "utool_AudioRT/Config.h"


// 对信号进行延时，现在效果好像不是很显著，我假设，当作他现在是可行的
void Data_Delay(std::vector<std::vector<__fp16>>& mag_H,
    const std::vector<std::vector<__fp16>>& phase_H,
    std::vector<std::vector<__fp16>>& mag_B,
    const std::vector<std::vector<__fp16>>& phase_B,
    std::vector<std::vector<__fp16>>& SPE_mag){
    // 先初始化参数
    SPE_mag.resize(2, std::vector<__fp16>(FFT_BINS)); // 固定就是最后2帧的IFFT
    size_t num_frames = mag_H.size(); // 动态获取mag的行数
    
    double Constant_Num = -2.0 * PI / FFT_BINS * SAMPLE_RATE / 2 * d_Mic/c_Sound; // 延时系数中的常数

    for (size_t i = num_frames - 2; i < num_frames; ++i) {
        for (size_t k = 0; k < FFT_BINS; ++k) {
            // S_H 复数谱
            std::complex<double> X_H = std::polar(static_cast<double>(mag_H[i][k]),
                                                  static_cast<double>(phase_H[i][k]*PI));
            
            // S_B 复数谱并加延时相移
            double phase_shift = Constant_Num * k;
            std::complex<double> X_B_delayed = std::polar(static_cast<double>(mag_B[i][k]),
                                                           static_cast<double>(phase_B[i][k]*PI) + phase_shift);
            // 复数谱相减
            std::complex<double> X_diff = X_H - X_B_delayed;

            // 输出幅度谱
            SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>(std::abs(X_diff));
        }
    }
}

// 先来复现师姐第二章的多竞争言语的算法复现，多竞争言语的降噪
void Decrease_Mutil_Speech(std::vector<std::vector<__fp16>>& mag_H,
    const std::vector<std::vector<__fp16>>& phase_H,
    std::vector<std::vector<__fp16>>& mag_B,
    const std::vector<std::vector<__fp16>>& phase_B,
    std::vector<std::vector<__fp16>>& SPE_mag){
    // 先初始化参数
    SPE_mag.resize(2, std::vector<__fp16>(FFT_BINS)); // 固定就是最后2帧的IFFT
    size_t num_frames = mag_H.size(); // 动态获取mag的行数
    __fp16 Omiga_0 = 2 * PI / FRAME_SIZE; // 常数项
    __fp16 Delt_1 = d_Mic / c_Sound * SAMPLE_RATE;
    for (size_t i = num_frames - 2; i < num_frames; ++i) {
        for (size_t k = 0; k < FFT_BINS; ++k) {
            // 构造复数谱
            std::complex<__fp16> X_H = std::polar(static_cast<__fp16>(mag_H[i][k]),
                                                  static_cast<__fp16>(phase_H[i][k]*PI)); // !!!!!这个相位是归一化的角度！！，要加PI!!!
            std::complex<__fp16> X_B = std::polar(static_cast<__fp16>(mag_B[i][k]),
                                                  static_cast<__fp16>(phase_B[i][k]*PI)); // !!!!!这个相位是归一化的角度！！，要加PI!!!
            // 复数除法
            std::complex<__fp16> ratio = X_B / X_H;
            // 复数对数
            std::complex<__fp16> log_ratio = std::log(ratio);
            __fp16 Tao = -std::imag(log_ratio) / ((k+1) * Omiga_0);
            __fp16 Result = std::abs(Tao - Delt_1);

            //进行处理
            if(Result <= 0.1*Delt_1){
                SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>(mag_H[i][k]); // 接近主要角度，就保留
            }
            else if ((Result <= 0.3*Delt_1) && (Result > 0.1*Delt_1))
            {
                double val = 10.0 * (Tao - 0.7 * Delt_1) / (0.2 * Delt_1);
                val = std::max(val, 1.0);
                val = std::log10(val);
                SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>(val * mag_H[i][k]);
                // SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>(0);
            }
            else{
                SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>(0);
            }
            // SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>(mag_H[i][k]);
        }
    }
}

// 来复现师姐第3章内容
void DeNoise_MutileSpeech(std::vector<std::vector<__fp16>>& mag_H,
    const std::vector<std::vector<__fp16>>& phase_H,
    std::vector<std::vector<__fp16>>& mag_B,
    const std::vector<std::vector<__fp16>>& phase_B,
    std::vector<std::vector<__fp16>>& SPE_mag,
    std::vector<std::vector<__fp16>>& SPE_pha){
    // 先初始化参数
    SPE_mag.resize(2, std::vector<__fp16>(FFT_BINS)); // 固定就是最后2帧的IFFT
    SPE_pha.resize(2, std::vector<__fp16>(FFT_BINS)); // 固定就是最后2帧的IFFT
    size_t num_frames = mag_H.size(); // 动态获取mag的行数
    __fp16 Omiga_0 = 2 * PI / FRAME_SIZE; // 常数项
    __fp16 Delt_1 = d_Mic / c_Sound * SAMPLE_RATE * Omiga_0; // 正方向--0度
    __fp16 Delt_2 = d_Mic / c_Sound * SAMPLE_RATE * 1/2 * Omiga_0; // 侧方向--60度
    __fp16 Delt_3 = -d_Mic / c_Sound * SAMPLE_RATE * 1/2 * Omiga_0; // 侧方向--120度
    double Constant_Num = -2.0 * PI / FFT_BINS * SAMPLE_RATE / 2 * d_Mic/c_Sound; // 延时系数中的常数
    // 测试变量
    int test = 0;
    for (size_t i = num_frames - 2; i < num_frames; ++i) {
        test = 1;
        for (size_t k = 0; k < FFT_BINS; ++k) {
            // S_H 复数谱
            std::complex<double> X_H = std::polar(static_cast<double>(mag_H[i][k]),
                                                  static_cast<double>(phase_H[i][k]*PI));
            // S_B 复数谱
            std::complex<double> X_B = std::polar(static_cast<double>(mag_B[i][k]),
                                                  static_cast<double>(phase_B[i][k]*PI));
            // S_H 复数谱并加延时相移
            double phase_shift = Constant_Num * k;
            std::complex<double> X_H_delayed = std::polar(static_cast<double>(mag_H[i][k]),
                                                           static_cast<double>(phase_H[i][k]*PI + phase_shift));
            // S_B 复数谱并加延时相移
            std::complex<double> X_B_delayed = std::polar(static_cast<double>(mag_B[i][k]),
                                                           static_cast<double>(phase_B[i][k]*PI + phase_shift));

            // 复数谱相减--CH1正向麦克风通道
            std::complex<double> X_CH1 = X_H - X_B_delayed;
            // 复数谱相减--CH2反向麦克风通道
            std::complex<double> X_CH2 = X_B - X_H_delayed;
            // 计算两个方向性通道的幅度比值
            double Ratio = std::abs(X_CH1)/(std::abs(X_CH2) + 1e-12);
            // 存储中间变量
            std::complex<double> STFT_M1m;
            std::complex<double> STFT_M2m;
            double LH_new1;
            double LH_new2;
            double LH_new3;
            // 测试变量
            double Sum_LH;
            double Target_C1;
            double Target_C2;
            // 根据Ratio的值进行掩蔽模板生成
            if(Ratio <= 1){
                // SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>(0.5* Ratio * Ratio * mag_H[i][k]);
                STFT_M1m = 0.5* Ratio * Ratio * X_H;
                STFT_M2m = 0.5* Ratio * Ratio * X_B;
            }
            else if((Ratio > 1) && (Ratio <= 2)){
                // SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>((1-0.5*(Ratio-2)*(Ratio-2)) * mag_H[i][k]);
                STFT_M1m = (1-0.5*(Ratio-2)*(Ratio-2)) * X_H;
                STFT_M2m = (1-0.5*(Ratio-2)*(Ratio-2)) * X_B;
            }
            else{
                // SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>(mag_H[i][k]);
                STFT_M1m = X_H;
                STFT_M2m = X_B;
            }
            // 接下来计算方向性参数
            std::complex<double> Dirc_Fac1 = std::polar(1.0,static_cast<double>(-Delt_1*(k+1)));
            std::complex<double> Dirc_Fac2 = std::polar(1.0,static_cast<double>(-Delt_2*(k+1)));
            std::complex<double> Dirc_Fac3 = std::polar(1.0,static_cast<double>(-Delt_3*(k+1)));
            LH_new1 = std::abs(STFT_M1m*Dirc_Fac1 - STFT_M2m); // 方向1
            LH_new2 = std::abs(STFT_M1m*Dirc_Fac2 - STFT_M2m); // 方向2
            LH_new3 = std::abs(STFT_M1m*Dirc_Fac3 - STFT_M2m); // 方向3
            // Sum_LH = LH_new1 + LH_new2 + LH_new3;
            // Target_C1 = 1 - LH_new1/Sum_LH;
            // Target_C2 = 1 - Target_C1;
            if (LH_new1 <= LH_new2 && LH_new1 <= LH_new3) {
                // LH_new1 最小
                // 在这里写 LH_new1 的操作
                SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>(std::abs(STFT_M1m));
                SPE_pha[i - num_frames + 2][k] = static_cast<__fp16>(std::arg(STFT_M1m)/PI);
                // 采用非固定的变量
                // SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>(std::abs(STFT_M1m)*Target_C1);
                // SPE_pha[i - num_frames + 2][k] = static_cast<__fp16>(std::arg(STFT_M1m)/PI*Target_C1);
            }
            else{
                SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>(0);
                SPE_pha[i - num_frames + 2][k] = static_cast<__fp16>(0);
                // 采用非固定的变量
                // SPE_mag[i - num_frames + 2][k] = static_cast<__fp16>(std::abs(STFT_M1m)*Target_C2);
                // SPE_pha[i - num_frames + 2][k] = static_cast<__fp16>(std::arg(STFT_M1m)/PI*Target_C2);
            }

        }
    }
}